/*
 *  obstacleMap.c
 *  rovercontrol08
 *
 *  Created by David Miller on 7/28/08.
 *  Copyright 2008 University of Oklahoma. All rights reserved.
 *
 */

#include <math.h>
#include "roverSide.h"
#include "obstacleMap.h"

//integer absolute value
int abs(int x){if(x<0)return(-x); else return(x);}
//returns 1 if obstacle is merged with existing obstacle, 0 if similarly placed obstacle does not already exist.
//checks active points to see if they should be deactivated
//note that it aonly checks the full list if there is not an obstacle that merges
int mergeObstacle(roverProperties *rover, Point3D obst)
{
	int i, x=(int)(10.0*obst.x), y=(int)(10.0*obst.y);
	int rx=(int)(10.0*rover->position.x), ry=(int)(10.0*rover->position.y);
	for(i=0;i<MAXNUMOBSTACLES;i++)
		if(Obstacles[i].active){
			if((abs(rx-Obstacles[i].xIdx) > rk.MAXDISTMAP) || (abs(ry-Obstacles[i].yIdx) > rk.MAXDISTMAP))
				Obstacles[i].active = 0; //deactivate out of range obstacles
			else{ // if active, does it match?
				if(x == Obstacles[i].xIdx)
					if(y == Obstacles[i].yIdx){
						if(fabs(obst.z) > fabs(Obstacles[i].position.z))
							Obstacles[i].position.z = obst.z;
						return (1);
					}
			}
		}
	return(0); // not merged
}
			
// this function deactivates obstacles out of range, tries to merge new obstacle with existing active obstacles
// and then tries to add this obstacle as a new obstacle to the map
// returns 0 if map is full, -1 if merged and 1 if added.
int addObstacle(roverProperties *rover, Point3D obst)
{
	int i, x=(int)(10.0*obst.x), y=(int)(10.0*obst.y);	
	
	if(!mergeObstacle(rover, obst)){ //deactivate obstacles that are too far away
		for(i=0;i<MAXNUMOBSTACLES;i++)
			if(!Obstacles[i].active){
				Obstacles[i].active=1;
				Obstacles[i].xIdx=x;
				Obstacles[i].yIdx=y;
				Obstacles[i].position=obst;
				return(1);
			}
		return(0); //not merged and no room for new obstacles
	}
	return(-1); // obstacle was sucessfully merged
}

int blockedCooridor(roverProperties *rover, float *distance)
{
	float frX=rover->position.x*10, frY=rover->position.y;
	float rHead,slope;
	int i, rX, rY;
	rX=(int)frX; rY=(int)frY;
	rHead=normalizeRadAngle(HALFPI-DEGTORAD(rover->compass));
	slope=tan(rHead);
	*distance = -100.0;
	if(rover->compass == 0.0)
		for(i=0;i<MAXNUMOBSTACLES;i++){
			if((Obstacles[i].yIdx > rY) && (Obstacles[i].xIdx > (rX-rk.ROVERWIDTHINT)) && (Obstacles[i].xIdx < (rX-rk.ROVERWIDTHINT)))
				(*distance > ((float)(Obstacles[i].yIdx-rY)/10.0))?*distance:((float)(Obstacles[i].yIdx-rY)/10.0);
		}
	else {
		if(rover->compass == 180.0)
			for(i=0;i<MAXNUMOBSTACLES;i++){
				if(Obstacles[i].yIdx < rY && Obstacles[i].xIdx > (rX-rk.ROVERWIDTHINT) && Obstacles[i].xIdx< (rX-rk.ROVERWIDTHINT))
					(*distance > -((float)(Obstacles[i].yIdx-rY)/10.0))?*distance:-((float)(Obstacles[i].yIdx-rY)/10.0);
		}
		else{
			
		}
	}
	return(999);//this is not done yet
}
